//
//  TTKalmanFilter.m
//  iBeaconDemo
//
//  Created by cry on 17/1/10.
//  Copyright © 2017年 eGova. All rights reserved.
//

#import "TTKalmanFilter.h"
#import "TTPoint.h"

@interface TTKalmanFilter()
/**
 * prioreEstimate is the prior Point which we get from the variable estimate
 * estimate is the new Point that is calculated
 * rawValue is used in order to store the lastPosition which is displayed
 */
//private Point priorEstimate, estimate, rawValue;
@property (nonatomic, strong) TTPoint *priorEstimate, *estimate, *rawValue;
/**.
 * priorErrorVariance is the prior ErrorVariance which we get from the variable errorCovariance
 * errorCovarRSSI is the newest estimate of the average error for each part of the state.
 * kalmanGain
 */
//private double priorErrorVariance,kalmanGain;
@property (nonatomic, assign) double priorErrorVariance, kalmanGain;
/**
 * firstTimeRunning check if the Filter is used first time
 */
//private boolean firstTimeRunning = true;
@property (nonatomic, assign) BOOL firstTimeRunning;
/**
 * sensorNose is the estimated measurement error covariance
 * processNoise is the estimated process error covariance
 */
//private static double sensorNoise, processNoise;
@property (nonatomic, assign) double sensorNoise, processNoise;
/**
 * priorRSSI is the previous calculated RSSI value.
 * estimateRSSI is the new calculate RSSI value
 * rawRSSI is temporary parameter to store the estimateRSSI
 */
//private double priorRSSI, estimateRSSI, rawRSSI;
@property (nonatomic, assign) double priorRSSI, estimateRSSI, rawRSSI;
/**
 * prioErrorCovarRSSI is the previous Covariance
 * errorCovarRSSI is the new calculated Covariance
 */
//private double priorErrorCovarRSSI,errorCovarRSSI;
@property (nonatomic, assign) double priorErrorCovarRSSI,errorCovarRSSI;
/**
 * firstTimeRun check if the Filter is used first time
 */
//private static boolean firstTimeRun = true;
@property (nonatomic, assign) BOOL firstTimeRun;

@end
/**
 * SensorNoise_DEFAUL is the estimated measurement error covariance as default value
 */
static double const SENSORNOISE_DEFAULT = 0.8;
/**
 * Processnoise_DEFAULT is the estimated process error covariance as default value
 */
static double const PROCESSNOISE_DEFAULT = 0.125;

@implementation TTKalmanFilter
/**
 * Default constructur
 */
- (instancetype)init{
    if (self = [super init]) {
        self.processNoise = PROCESSNOISE_DEFAULT;
        self.sensorNoise = SENSORNOISE_DEFAULT;
        self.priorErrorVariance = 1;
        self.firstTimeRun = YES;
        self.firstTimeRunning = YES;
    }
    return self;
}

- (instancetype)initWithProcessNoise:(double)processNoise
                         sensorNoise:(double)sensorNoise{
    if (self = [super init]) {
        self.processNoise = processNoise;
        self.sensorNoise = sensorNoise;
        self.priorErrorVariance = 1;
        self.firstTimeRun = YES;
        self.firstTimeRunning = YES;
    }
    return self;
}

/**
 * @param lastPosition is the position we want to estimate better
 * @return A new point that represents the position of the watch
 */
//@Override
//public Point applyFilter(Point lastPosition) {
//    
//    if (firstTimeRunning) {
//        //estimate is the old one here
//        priorEstimate = lastPosition;
//        firstTimeRunning = false;
//    }
//    
//    //Prediction Part
//    else {
//        //estimate is the old one here
//        priorEstimate = estimate;
//        //errorCovariance is the old one
//        priorErrorVariance = errorCovarRSSI + processNoise;
//        
//    }
//    //Correction Part
//    //lastPosition is the newest Position recieved
//    rawValue = lastPosition;
//    kalmanGain = priorErrorVariance / (priorErrorVariance + sensorNoise);
//    estimate = new Point(priorEstimate.getX() + (kalmanGain * (rawValue.getX() - priorEstimate.getX())), priorEstimate.getY() + (kalmanGain * (rawValue.getY() - priorEstimate.getY())));
//    errorCovarRSSI = (1 - kalmanGain) * priorErrorVariance;
//    //posistion is the variable I want to update which will be lastPosition next time
//    lastPosition = new Point(estimate.getX(), 	estimate.getY());
//    
//    return lastPosition;
//}
- (TTPoint *)applyFilterWithPiont:(TTPoint *)point{
    
    if (_firstTimeRunning) {
        _priorEstimate = point;
        _firstTimeRunning = NO;
    }else{
        _priorEstimate = _estimate;
        _priorErrorVariance = _errorCovarRSSI + _processNoise;
    }
    _rawValue = point;
    _kalmanGain = _priorErrorVariance / (_priorErrorVariance + _sensorNoise);
    _estimate = [[TTPoint alloc] initWithX:_priorEstimate.x + (_kalmanGain * (_rawValue.x - _priorEstimate.x))
                                         Y:_priorEstimate.y + (_kalmanGain * (_rawValue.y - _priorEstimate.y))];
    _errorCovarRSSI = (1 - _kalmanGain) *_priorErrorVariance;
    return [[TTPoint alloc] initWithX:_estimate.x Y:_estimate.y];
}

/**
 * @param rssi is the RSSI value we want to estimate better
 * @return A new RSSI value which is calculated with the kalman filter based on the previous RSSI value
 */
//public double updateRSSI (double rssi) {
- (double)updateRSSI:(double)rssi{
//    if (firstTimeRun) {
//        priorRSSI = rssi;
//        priorErrorCovarRSSI = 0.5;
//        firstTimeRun = false;
//    }
    if (self.firstTimeRun) {
        self.priorRSSI = rssi;
        self.priorErrorCovarRSSI = 0.5;
        self.firstTimeRun = NO;
    }
//    
//    //Prediction Part
//    
//    else {
//        priorRSSI = estimateRSSI;
//        priorErrorCovarRSSI = errorCovarRSSI + processNoise;
//    }
    else{
        self.priorRSSI = self.estimateRSSI;
        self.priorErrorCovarRSSI = self.errorCovarRSSI + self.processNoise;
    }
//    //Correction Part
//    
//    rawRSSI = rssi;
//    kalmanGain = priorErrorCovarRSSI / (priorErrorCovarRSSI + sensorNoise);
//    estimateRSSI = priorRSSI + (kalmanGain * (rawRSSI - priorRSSI));
//    errorCovarRSSI = (1 - kalmanGain) * priorErrorCovarRSSI;
//    rssi = estimateRSSI;
    self.rawRSSI = rssi;
    self.kalmanGain = self.priorErrorCovarRSSI / (self.priorErrorCovarRSSI + self.sensorNoise);
    self.estimateRSSI = self.priorRSSI + (self.kalmanGain * (self.rawRSSI - self.priorRSSI));
    rssi = self.estimateRSSI;
    return rssi;
}
@end
